#include <diffbot_base/diffbot_hw_interface.h>

// ROS参数加载
#include <rosparam_shortcuts/rosparam_shortcuts.h>

//#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Empty.h>

#include <iomanip>
 
namespace diffbot_base
{
    DiffBotHWInterface::DiffBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model)
        : name_("hardware_interface")
        , nh_(nh)
    { 
        // 可以在这里或init()中初始化机器人的资源(关节、传感器、执行器)和接口
        // 例如:解析URDF获取关节名称和接口,然后初始化它们
        // 检查是否需要加载URDF模型
        if (urdf_model == NULL)
            loadURDF(nh, "robot_description");
        else
            urdf_model_ = urdf_model;

        // 加载ROS参数
        ros::NodeHandle rpnh(nh_, name_);
        std::size_t error = 0;
        // rosparam_shortcuts的代码API:
        // http://docs.ros.org/en/noetic/api/rosparam_shortcuts/html/namespacerosparam__shortcuts.html#aa6536fe0130903960b1de4872df68d5d
        error += !rosparam_shortcuts::get(name_, rpnh, "joints", joint_names_);
        error += !rosparam_shortcuts::get(name_, nh_, "mobile_base_controller/wheel_radius", wheel_radius_);
        error += !rosparam_shortcuts::get(name_, nh_, "mobile_base_controller/linear/x/max_velocity", max_velocity_);
        // 从参数服务器上的diffbot_base/config/base.yaml获取额外参数
        error += !rosparam_shortcuts::get(name_, nh_, "encoder_resolution", encoder_resolution_);
        error += !rosparam_shortcuts::get(name_, nh_, "gain", gain_);
        error += !rosparam_shortcuts::get(name_, nh_, "trim", trim_);
        error += !rosparam_shortcuts::get(name_, nh_, "motor_constant", motor_constant_);
        error += !rosparam_shortcuts::get(name_, nh_, "pwm_limit", pwm_limit_);
        error += !rosparam_shortcuts::get(name_, nh_, "debug/hardware_interface", debug_);
        rosparam_shortcuts::shutdownIfError(name_, error);

        wheel_diameter_ = 2.0 * wheel_radius_;
        //max_velocity_ = 0.2; // m/s
        // ros_control RobotHW需要rad/s的速度,但配置中给出的是m/s
        max_velocity_ = linearToAngular(max_velocity_);


        ROS_INFO_STREAM("mobile_base_controller/wheel_radius: " << wheel_radius_);
        ROS_INFO_STREAM("mobile_base_controller/linear/x/max_velocity: " << max_velocity_);
        ROS_INFO_STREAM("encoder_resolution: " << encoder_resolution_);
        ROS_INFO_STREAM("gain: " << gain_);
        ROS_INFO_STREAM("trim: " << trim_);
        ROS_INFO_STREAM("motor_constant: " << motor_constant_);
        ROS_INFO_STREAM("pwm_limit: " << pwm_limit_);

        // 设置电机驱动器的发布者
        pub_left_motor_value_ = nh_.advertise<std_msgs::Int32>("motor_left", 10);
        pub_right_motor_value_ = nh_.advertise<std_msgs::Int32>("motor_right", 10);

        // 设置轮子角速度命令的发布者
        pub_wheel_cmd_velocities_ = nh_.advertise<diffbot_msgs::WheelsCmdStamped>("wheel_cmd_velocities", 10);

        // 设置重置编码器的发布者(在硬件接口首次启动时使用)
        pub_reset_encoders_ = nh_.advertise<std_msgs::Empty>("reset", 10);
        // 设置轮子编码器的订阅者
        sub_encoder_ticks_ = nh_.subscribe("encoder_ticks", 10, &DiffBotHWInterface::encoderTicksCallback, this);
        sub_measured_joint_states_ = nh_.subscribe("measured_joint_states", 10, &DiffBotHWInterface::measuredJointStatesCallback, this);

        // 初始化硬件接口
        init(nh_, nh_);

        // 等待编码器消息发布
        isReceivingMeasuredJointStates(ros::Duration(10));
    }

 
    bool DiffBotHWInterface::init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh)
    {
        ROS_INFO("\033[1;32m---->Initializing DiffBot Hardware Interface... \033[0m");
        num_joints_ = joint_names_.size();
        ROS_INFO("Number of joints: %d", (int)num_joints_);
        std::array<std::string, NUM_JOINTS> motor_names = {"left_motor", "right_motor"};
        for (unsigned int i = 0; i < num_joints_; i++)
        {
            // 为每个关节创建JointStateHandle并注册到JointStateInterface
            hardware_interface::JointStateHandle joint_state_handle(joint_names_[i],
                                                                    &joint_positions_[i], 
                                                                    &joint_velocities_[i],
                                                                    &joint_efforts_[i]);
            joint_state_interface_.registerHandle(joint_state_handle);

            // 为每个可控关节创建JointHandle(读写),使用JointStateInterface中的只读关节句柄,
            // 并注册到JointVelocityInterface
            hardware_interface::JointHandle joint_handle(joint_state_handle, &joint_velocity_commands_[i]);
            velocity_joint_interface_.registerHandle(joint_handle);

            // 用零值初始化关节状态
            joint_positions_[i] = 0.0;
            joint_velocities_[i] = 0.0;
            joint_efforts_[i] = 0.0; // diff_drive_controller未使用

            joint_velocity_commands_[i] = 0.0;

            // 将encoder_ticks_初始化为零,因为从微控制器接收有意义的
            // tick值可能需要一些时间
            encoder_ticks_[i] = 0.0;
            measured_joint_states_[i].angular_position_ = 0.0;
            measured_joint_states_[i].angular_velocity_ = 0.0;

            // 使用机器人命名空间初始化电机的PID控制器
            std::string pid_namespace = "pid/" + motor_names[i];
            ROS_INFO_STREAM("pid namespace: " << pid_namespace);
            ros::NodeHandle nh(root_nh, pid_namespace);
            // TODO 实现构建器模式来初始化值,否则很难看出哪个参数是什么
            pids_[i].init(nh, 0.8, 0.35, 0.5, 0.01, 3.5, -3.5, false, max_velocity_, -max_velocity_);
            pids_[i].setOutputLimits(-max_velocity_, max_velocity_);
        }

        // 向hardware_interface::RobotHW注册包含只读关节的JointStateInterface
        registerInterface(&joint_state_interface_);

        // 向hardware_interface::RobotHW注册包含读写关节的JointVelocityInterface
        registerInterface(&velocity_joint_interface_);

        ROS_INFO("\033[36m ... Done Initializing DiffBot Hardware Interface\033[0m");

        return true;
    }

    void DiffBotHWInterface::read(const ros::Time& time, const ros::Duration& period)
    {
        //ROS_INFO_THROTTLE(1, "读取");
        ros::Duration elapsed_time = period;

        // 从机器人硬件读取(电机编码器)
        // 用读取的值填充joint_state_*成员
        for (std::size_t i = 0; i < num_joints_; ++i)
        {
            joint_positions_[i] = measured_joint_states_[i].angular_position_;
            joint_velocities_[i] = measured_joint_states_[i].angular_velocity_;
            joint_efforts_[i] = 0.0; // diff_drive_controller未使用
        }

        if (debug_)
        {
            const int width = 10;
            const char sep = ' ';
            std::stringstream ss;
            ss << std::left << std::setw(width) << std::setfill(sep) << "读取" << std::left << std::setw(width) << std::setfill(sep) << "ticks" << std::left << std::setw(width) << std::setfill(sep) << "角度" << std::left << std::setw(width) << std::setfill(sep) << "速度" << std::endl;
            ss << std::left << std::setw(width) << std::setfill(sep) << "j0:" << std::left << std::setw(width) << std::setfill(sep) << encoder_ticks_[0] << std::left << std::setw(width) << std::setfill(sep) << joint_positions_[0] << std::left << std::setw(width) << std::setfill(sep) << joint_velocities_[0] << std::endl;
            ss << std::left << std::setw(width) << std::setfill(sep) << "j1:" << std::left << std::setw(width) << std::setfill(sep) << encoder_ticks_[1] << std::left << std::setw(width) << std::setfill(sep) << joint_positions_[1] << std::left << std::setw(width) << std::setfill(sep) << std::setfill(sep) << joint_velocities_[1];
            ROS_INFO_STREAM(std::endl << ss.str());
            //printState();
        }
    }

    void DiffBotHWInterface::write(const ros::Time& time, const ros::Duration& period)
    {
        ros::Duration elapsed_time = period;
        // 写入机器人硬件
        // ros_control的RobotHW的关节速度命令单位为rad/s

        // 根据增益和修正调整k
        double motor_constant_right_inv = (gain_ + trim_) / motor_constant_;
        double motor_constant_left_inv = (gain_ - trim_) / motor_constant_;


        joint_velocity_commands_[0] = joint_velocity_commands_[0] * motor_constant_left_inv;
        joint_velocity_commands_[1] = joint_velocity_commands_[1] * motor_constant_right_inv;


        // 发布期望的(命令的)轮子角速度
        diffbot_msgs::WheelsCmdStamped wheel_cmd_msg;
        wheel_cmd_msg.header.stamp = ros::Time::now();
        for (int i = 0; i < NUM_JOINTS; ++i)
        {
            wheel_cmd_msg.wheels_cmd.angular_velocities.joint.push_back(joint_velocity_commands_[i]);
        }

        pub_wheel_cmd_velocities_.publish(wheel_cmd_msg);

        // 以下代码提供了另一个速度命令接口
        // 通过它,电机驱动节点可以直接订阅期望的速度

        // 将速度命令转换为电机的百分比值
        // 这将速度映射到百分比值,用于对每个电机施加最大可能电池电压的百分比
        std_msgs::Int32 left_motor;
        std_msgs::Int32 right_motor;

        double pid_outputs[NUM_JOINTS];
        double motor_cmds[NUM_JOINTS] ;
        pid_outputs[0] = pids_[0](joint_velocities_[0], joint_velocity_commands_[0], period);
        pid_outputs[1] = pids_[1](joint_velocities_[1], joint_velocity_commands_[1], period);

        motor_cmds[0] = pid_outputs[0] / max_velocity_ * 100.0;
        motor_cmds[1] = pid_outputs[1] / max_velocity_ * 100.0;
        left_motor.data = motor_cmds[0];
        right_motor.data = motor_cmds[1];

        // 校准电机命令以处理左右电机的不同齿轮摩擦和轮子可能的差异
        // 在低区域添加校准偏移
        // 要调整这些偏移值,命令机器人直线行驶并调整,如果它不是直线行驶
        // int left_offset = 10;
        // int right_offset = 5;
        // int threshold = 55;
        // if (0 < left_motor.data && left_motor.data < threshold)
        // {
        //     // 乘法的第二部分让偏移随电机值增大而减小
        //     left_motor.data += left_offset * (threshold - left_motor.data) / threshold;
        // }
        // if (0 < right_motor.data && right_motor.data < threshold)
        // {
        //     // 乘法的第二部分让偏移随电机值增大而减小
        //     right_motor.data += right_offset * (threshold - right_motor.data) / threshold;
        // }

        pub_left_motor_value_.publish(left_motor);
        pub_right_motor_value_.publish(right_motor);


        if (debug_)
        {
            const int width = 10;
            const char sep = ' ';
            std::stringstream ss;
            // 标题
            ss << std::left << std::setw(width) << std::setfill(sep) << "写入"
            << std::left << std::setw(width) << std::setfill(sep) << "速度"
            << std::left << std::setw(width) << std::setfill(sep) << "p_error"
            << std::left << std::setw(width) << std::setfill(sep) << "i_error"
            << std::left << std::setw(width) << std::setfill(sep) << "d_error"
            << std::left << std::setw(width) << std::setfill(sep) << "pid输出"
            << std::left << std::setw(width) << std::setfill(sep) << "百分比"
            << std::endl;
            double p_error, i_error, d_error;
            for (int i = 0; i < NUM_JOINTS; ++i)
            {
                pids_[i].getCurrentPIDErrors(&p_error, &i_error, &d_error);

                // 关节i
                std::string j = "j" + std::to_string(i) + ":";
                ss << std::left << std::setw(width) << std::setfill(sep) << j
                << std::left << std::setw(width) << std::setfill(sep) << joint_velocity_commands_[i]
                << std::left << std::setw(width) << std::setfill(sep) << p_error
                << std::left << std::setw(width) << std::setfill(sep) << i_error
                << std::left << std::setw(width) << std::setfill(sep) << d_error
                << std::left << std::setw(width) << std::setfill(sep) << pid_outputs[i]
                << std::left << std::setw(width) << std::setfill(sep) << motor_cmds[i]
                << std::endl;
            }
            ROS_INFO_STREAM(std::endl << ss.str());
        }
    }

    bool DiffBotHWInterface::isReceivingMeasuredJointStates(const ros::Duration &timeout)
    {
        ROS_INFO("Get number of measured joint states publishers");

        ros::Time start = ros::Time::now();
        int num_publishers = sub_measured_joint_states_.getNumPublishers();
        ROS_INFO("Waiting for measured joint states being published...");
        while ((num_publishers == 0) && (ros::Time::now() < start + timeout))
        {
            ros::Duration(0.1).sleep();
            num_publishers = sub_measured_joint_states_.getNumPublishers();
        }
        if (num_publishers == 0)
        {
            ROS_ERROR("No measured joint states publishers. Timeout reached.");
        }
        else
        {
            ROS_INFO_STREAM("Number of measured joint states publishers: " << num_publishers);
        }

        ROS_INFO("Publish /reset to encoders");
        std_msgs::Empty msg;
        pub_reset_encoders_.publish(msg);

        return (num_publishers > 0);
    }

    void DiffBotHWInterface::loadURDF(const ros::NodeHandle &nh, std::string param_name)
    {
        std::string urdf_string;
        urdf_model_ = new urdf::Model();

        // 在参数服务器上搜索并等待robot_description
        while (urdf_string.empty() && ros::ok())
        {
            std::string search_param_name;
            if (nh.searchParam(param_name, search_param_name))
            {
                ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " <<
                                        nh.getNamespace() << search_param_name);
                nh.getParam(search_param_name, urdf_string);
            }
            else
            {
                ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " <<
                                        nh.getNamespace() << param_name);
                nh.getParam(param_name, urdf_string);
            }

            usleep(100000);
        }

        if (!urdf_model_->initString(urdf_string))
            ROS_ERROR_STREAM_NAMED(name_, "Unable to load URDF model");
        else
            ROS_DEBUG_STREAM_NAMED(name_, "Received URDF from param server");
    }

    void DiffBotHWInterface::printState()
    {
        // 警告:这不是实时安全的
        // 仅用于调试,使用风险自负!
        ROS_INFO_STREAM_THROTTLE(1, std::endl << printStateHelper());
    }

    std::string DiffBotHWInterface::printStateHelper()
    {
        std::stringstream ss;
        std::cout.precision(15);

        for (std::size_t i = 0; i < num_joints_; ++i)
        {
            ss << "j" << i << ": " << std::fixed << joint_positions_[i] << "\t ";
            ss << std::fixed << joint_velocities_[i] << "\t ";
            ss << std::fixed << joint_efforts_[i] << std::endl;
        }
        return ss.str();
    }

    std::string DiffBotHWInterface::printCommandHelper()
    {
        std::stringstream ss;
        std::cout.precision(15);
        // ss << "    位置     速度         力矩  \n";
        ss << "    position     velocity         effort  \n";

        for (std::size_t i = 0; i < num_joints_; ++i)
        {
            ss << std::fixed << joint_velocity_commands_[i] << "\t ";
        }
        return ss.str();
    }


    /// 处理编码器更新
    void DiffBotHWInterface::encoderTicksCallback(const diffbot_msgs::EncodersStamped::ConstPtr& msg_encoder)
    {
        /// 更新编码器数组中的当前编码器计数
        encoder_ticks_[0] = msg_encoder->encoders.ticks[0];
        encoder_ticks_[1] = msg_encoder->encoders.ticks[1];
        ROS_DEBUG_STREAM_THROTTLE(1, "左编码器计数: " << encoder_ticks_[0]);
        ROS_DEBUG_STREAM_THROTTLE(1, "右编码器计数: " << encoder_ticks_[1]);
    }

    void DiffBotHWInterface::measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states)
    {
        /// 更新编码器数组中的当前编码器计数
        for (std::size_t i = 0; i < num_joints_; ++i)
        {
            measured_joint_states_[i].angular_position_ = msg_joint_states->position[i];
            measured_joint_states_[i].angular_velocity_ = msg_joint_states->velocity[i];
        }
        //ROS_DEBUG_STREAM_THROTTLE(1, "左编码器计数: " << encoder_ticks_[0]);
        //ROS_DEBUG_STREAM_THROTTLE(1, "右编码器计数: " << encoder_ticks_[1]);
    }


    double DiffBotHWInterface::ticksToAngle(const int &ticks) const
    {
        // 将编码器计数转换为弧度角度
        double angle = (double)ticks * (2.0*M_PI / encoder_resolution_);
        ROS_DEBUG_STREAM_THROTTLE(1, ticks << " 计数对应角度 " << angle);
	    return angle;
    }

    double DiffBotHWInterface::normalizeAngle(double &angle) const
    {
        // https://stackoverflow.com/questions/11498169/dealing-with-angle-wrap-in-c-code
        angle = fmod(angle, 2.0*M_PI);

        if (angle < 0)
            angle += 2.0*M_PI;

        ROS_DEBUG_STREAM_THROTTLE(1, "归一化角度: " << angle);
        return angle;
    }


    double DiffBotHWInterface::linearToAngular(const double &distance) const
    {
        return distance / wheel_diameter_ * 2.0;
    }

    double DiffBotHWInterface::angularToLinear(const double &angle) const
    {
        return angle * wheel_diameter_ / 2.0;
    }

};
